/**
  ******************************************************************************
  * @file   step_run_table.c   
  * @brief  步进电机运动表
  * @author Moon
  * @version V1.0.0
  * @date 2022/09/04
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "step_run_table.h"
#include <math.h>
/* Private defines ---------------------------------------------------------------*/
#define SADAPT_ACC_LIST_LEN sizeof(SPlotAccSpeedAdapt_List) / sizeof(SPlotAccSpeedAdapt_List[0])
#define SADAPT_REDUCE_LIST_LEN sizeof(SPlotReduceSpeedAdapt_List) / sizeof(SPlotReduceSpeedAdapt_List[0])
#define UADAPT_SPEED_LIST_LEN sizeof(UniformSpeedAdapt_List) / sizeof(UniformSpeedAdapt_List[0])

typedef struct
{
	float flexible;	   // S型加减速曲线系数，一般4-6之间
	float start_speed; // S型加减速起始速度，单位RPM（每分钟多少转）
	uint32_t sum;	   //加速脉冲数
} SPlotAdaptParaDef_t;

typedef struct
{
	float head_speed;
	float tail_speed;
	SPlotAdaptParaDef_t SPlotAdaptPara;
} SPlotSpeedAdaptDef_t;

typedef struct
{
	float head_distance;
	float tail_distance;
	float uniform_speed;
} UniformSpeedAdaptDef_t;

/* Private Struct ---------------------------------------------------------------*/
SPlotSpeedAdaptDef_t SPlotAccSpeedAdapt_List[] = {
		//S型加速参数对速度表    包含头，不包括尾
		//head_speed  tail_speed          SPlotAdaptPara              //
		//                           flexible start_speed    sum	     //
		//区间头速度     尾速度   S型参数 曲线系数  起始速度    加减速脉冲数 //
		{0, 20, {6, 0.05, 50}},
		{20, 40, {6, 0.05, 100}},
		{40, 80, {6, 0.05, 150}},
		{80, 100, {6, 0.05, 200}},
		{100, 300, {6, 0.05, 250}},
};

SPlotSpeedAdaptDef_t SPlotReduceSpeedAdapt_List[] = {
		//S型减速参数对速度表    包含头，不包括尾
		{0, 20, {6, 0.05, 50}},
		{20, 40, {6, 0.05, 100}},
		{40, 80, {6, 0.05, 150}},
		{80, 100, {6, 0.05, 200}},
		{100, 300, {6, 0.05, 250}},
};

UniformSpeedAdaptDef_t UniformSpeedAdapt_List[] = {
		//匀速运动时，在不丢步情况下，对应的匀速速度   包含头，不包括尾
		//head_distance  tail_distance  uniform_speed
		//区间头运动距离    尾运动距离    匀速运行速度
		{0.0, 0.01, 5},
		{0.1, 0.2, 10},
		{0.2, 0.3, 15},
};

/* Private enum ---------------------------------------------------------------*/

/* Private Variable ---------------------------------------------------------------*/
// static uint16_t ACC_Curve_Num = 0;			//加速扫描次数
// static uint16_t Reduce_Curve_Num = 0;	//加速扫描次数
// static uint16_t Uniform_Curve_Num = 0; //匀速扫描次数
// uint32_t curve_count;									//当前已经执行的脉冲数

//Pointer

//Array

//Const

/* Private function prototypes -----------------------------------------------*/

/***********************************************************************************
 * @brief 查表法：在不同转速下，不丢步情况下，S型加减速对应的加速脉冲数
 * ex:
 * @par 
 * None    
 * @retval  
 **********************************************************************************/
SPlotAdaptParaDef_t *SPlotAccSpeedAdapt_Search(SPlotSpeedAdaptDef_t array[], float value) //包含头，不包括尾，列表未定义的以最后一个为标准
{
	int i;
	for (i = 0; i < SADAPT_ACC_LIST_LEN; i++)
	{
		if ((array[i].head_speed <= value) && (value < array[i].tail_speed))
			return &array[i].SPlotAdaptPara;
	}

	return &array[SADAPT_ACC_LIST_LEN - 1].SPlotAdaptPara;
	
	
}
/***********************************************************************************
 * @brief 查表找不同转速下，不丢步情况下，S型加减速对应的加速脉冲数
 * ex:
 * @par 
 * None    
 * @retval  
 **********************************************************************************/
SPlotAdaptParaDef_t *SPlotReduceSpeedAdapt_Search(SPlotSpeedAdaptDef_t array[], float value) //包含头，不包括尾，列表未定义的以最后一个为标准
{
	int i;
	for (i = 0; i < SADAPT_REDUCE_LIST_LEN; i++)
	{
		if ((array[i].head_speed <= value) && (value < array[i].tail_speed))
			return &array[i].SPlotAdaptPara;
	}

	return &array[SADAPT_REDUCE_LIST_LEN - 1].SPlotAdaptPara;
	
}
/***********************************************************************************
 * @brief 查表找不同转速下，不丢步情况下，S型加减速对应的加速脉冲数
 * ex:
 * @par 
 * None    
 * @retval  
 **********************************************************************************/
float UniformSpeedAdapt_Search(UniformSpeedAdaptDef_t array[], float value) //包含头，不包括尾，列表未定义的以最后一个为标准
{
	int i;
	for (i = 0; i < SADAPT_REDUCE_LIST_LEN; i++)
	{
		if ((array[i].head_distance <= value) && (value < array[i].tail_distance))
			return array[i].uniform_speed;
	}
	if (i >= SADAPT_REDUCE_LIST_LEN)
	{
		return array[SADAPT_REDUCE_LIST_LEN - 1].uniform_speed;
	}
	return 0;
}


/***********************************************************************************
 * @brief 绘制S型加速曲线
 * ex:       详见加减速表
 * @par 
 * None    
 * @retval void None
 **********************************************************************************/
void Plot_S_AccCurve(func_StepMotor_t *step_motor, float speed_rpm)
{
	float speed_temp = 0;
	uint32_t freq = 0;
	uint32_t freq_min = 0;
	uint32_t freq_max = 0;

	float deno;
	float melo;
	uint16_t ACC_Curve_Num = 0;			//加速扫描次数
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;
	if (speed_rpm == 0)
	{
		return;
	}

	speed_temp = rpm_turn_speed(step_motor, acc->ACC_start_speed);
	freq_min = speed_turn_freq(step_motor, speed_temp);

	speed_temp = rpm_turn_speed(step_motor, speed_rpm);
	freq_max = speed_turn_freq(step_motor, speed_temp);

	ACC_Curve_Num = acc->ACC_Curve_Sum;

	for (uint16_t ii = 0; ii < ACC_Curve_Num; ii++)
	{
		melo = acc->ACC_flexible * (ii - ACC_Curve_Num / 2) / (ACC_Curve_Num / 2);
		deno = 1.0 / (1 + exp(-melo));
		freq = (freq_max - freq_min) * deno + freq_min;

		acc->ACC_Curve_List[ii].speed = freq;
		acc->ACC_Curve_List[ii].cnt = 1; //每次扫描加速脉冲运行1次
	}
}

/***********************************************************************************
 * @brief 绘制S型减速曲线
 * ex:       详见加减速表
 * @par 
 * None    
 * @retval void None
 **********************************************************************************/
void Plot_S_ReduceCurve(func_StepMotor_t *step_motor, float speed_rpm)
{
	float speed_temp = 0;
	uint32_t freq = 0;
	uint32_t freq_min = 0;
	uint32_t freq_max = 0;

	float deno;
	float melo;

	uint16_t Reduce_Curve_Num = 0;
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;
	if (speed_rpm == 0)
	{
		return;
	}

	speed_temp = rpm_turn_speed(step_motor, acc->Reduce_end_speed);
	freq_min = speed_turn_freq(step_motor, speed_temp);

	speed_temp = rpm_turn_speed(step_motor, speed_rpm);
	freq_max = speed_turn_freq(step_motor, speed_temp);

	Reduce_Curve_Num = acc->Reduce_Curve_Sum;

	for (uint16_t ii = 0; ii < Reduce_Curve_Num; ii++)
	{
		melo = acc->Reduce_flexible * (ii - Reduce_Curve_Num / 2) / (Reduce_Curve_Num / 2);
		deno = 1.0 / (1 + exp(-melo));
		freq = freq_max - (freq_max - freq_min) * deno;

		acc->Reduce_Curve_List[ii].speed = freq;
		acc->Reduce_Curve_List[ii].cnt = 1;
	}
}

/***********************************************************************************
 * @brief 清空运动控制中间变量
 * ex:    
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
void Clearing_SportCurve_value(func_StepMotor_t *step_motor)
{
	step_motor->private.MotorAcc.curve_count = 0;
}

/***********************************************************************************
 * @brief 加载曲线
 * ex:    
 * @par speed:速度
 * None
 * @retval void None
 **********************************************************************************/
void Loading_SportCurve(func_StepMotor_t *step_motor, uint8_t type)
{
	Clearing_SportCurve_value(step_motor);
}

/***********************************************************************************
 * @brief 绘制T型匀速表
 * ex:    
 * @par speed:速度
 * None
 * @retval void None
 **********************************************************************************/
void Plot_T_UniformCurve(func_StepMotor_t *step_motor, uint32_t pluse_num, float speed_rpm)
{
	uint32_t freq = 0;
	uint16_t ii = 0;
	uint16_t Uniform_Curve_Num = 0; //匀速扫描次数
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;


	freq = rpm_turn_freq(step_motor, speed_rpm);

	Uniform_Curve_Num = (uint32_t)(pluse_num / step_motor->scan_cnt);

	if (Uniform_Curve_Num > UNIFORM_CURVE_MAX_NUMBER)
	{
		Uniform_Curve_Num = UNIFORM_CURVE_MAX_NUMBER;
	}
	for (; ii < Uniform_Curve_Num; ii++)
	{

		acc->Uniform_Curve_List[ii].speed = freq;
		acc->Uniform_Curve_List[ii].cnt = step_motor->scan_cnt;
		if (acc->Uniform_Curve_List[ii].cnt == 0)
		{
			acc->Uniform_Curve_List[ii].cnt++;
		}
	}
	if ((pluse_num - (step_motor->scan_cnt * Uniform_Curve_Num)) != 0)
	{
		acc->Uniform_Curve_List[ii].speed = freq; 
		acc->Uniform_Curve_List[ii].cnt = pluse_num - (step_motor->scan_cnt * Uniform_Curve_Num);
		if (acc->Uniform_Curve_List[ii].cnt == 0)
		{
			return;
		}
		Uniform_Curve_Num++;
	}
	acc->Uniform_Curve_Sum = Uniform_Curve_Num;
	if (acc->Uniform_Curve_Sum > UNIFORM_CURVE_MAX_NUMBER)
	{
		acc->Uniform_Curve_Sum = UNIFORM_CURVE_MAX_NUMBER;
	}
}

/***********************************************************************************
 * @brief S型加速曲线脉冲输出
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
uint32_t S_AccCurve_Sport(func_StepMotor_t *step_motor)
{
	static uint32_t curve_index; //扫描脉冲索引
	uint16_t curve_count_cnt;
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;
	
	if (acc->curve_count == 0) //首次进入加速过程，加速表下标清零
	{
		curve_index = 0;
	}

	step_motor->res_set_step_run(acc->ACC_Curve_List[curve_index].speed,
							 acc->ACC_Curve_List[curve_index].cnt);
	curve_count_cnt = acc->ACC_Curve_List[curve_index].cnt;

	step_motor->private.rel_detec_pulse_tar -= curve_count_cnt;

	curve_index += curve_count_cnt; //更新运动曲线下标
	if (curve_index >= acc->ACC_Curve_Sum)
	{
		curve_index = acc->ACC_Curve_Sum - 1;
	}
	return curve_count_cnt; //更新脉冲计数变量
}

/***********************************************************************************
 * @brief 匀速曲线输出
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
uint32_t UniformCurve_Sport(func_StepMotor_t *step_motor)
{
	static uint32_t curve_index; //扫描脉冲索引
	uint16_t curve_count_cnt;
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;

	if (acc->curve_count == acc->ACC_Curve_Sum || acc->curve_count == 0) //首次进入减速过程，加速表下标清零
	{
		curve_index = 0;
	}

	step_motor->res_set_step_run(acc->Uniform_Curve_List[curve_index].speed,
							 acc->Uniform_Curve_List[curve_index].cnt);

	curve_count_cnt = acc->Uniform_Curve_List[curve_index].cnt;

	step_motor->private.rel_detec_pulse_tar -= curve_count_cnt;
	curve_index = curve_index + 1;
	if (curve_index >= acc->Uniform_Curve_Sum)
	{
		curve_index = acc->Uniform_Curve_Sum - 1;
	}
	return curve_count_cnt; //更新脉冲计数变量
}

/***********************************************************************************
 * @brief S型减速曲线脉冲输出
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
uint32_t S_ReduceCurve_Sport(func_StepMotor_t *step_motor)
{
	static uint32_t curve_index; //扫描脉冲索引
	uint16_t curve_count_cnt;
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;
	if (acc->curve_count ==
		(step_motor->private.rel_pulse_tar - acc->Reduce_Curve_Sum)) //首次进入减速过程，加速表下标清零
	{
		curve_index = 0;
	}

	step_motor->res_set_step_run(acc->Reduce_Curve_List[curve_index].speed,
								 acc->Reduce_Curve_List[curve_index].cnt);
	curve_count_cnt = acc->Reduce_Curve_List[curve_index].cnt;

	step_motor->private.rel_detec_pulse_tar -= curve_count_cnt;

	curve_index += curve_count_cnt; //更新运动曲线下标
	if (curve_index >= acc->Reduce_Curve_Sum)
	{
		curve_index = acc->Reduce_Curve_Sum - 1;
	}
	return curve_count_cnt; //更新脉冲计数变量
}

/***********************************************************************************
 * @brief S型加速曲线脉冲输出
 * ex:
 * @par 
 * None
 * @retval void None
 **********************************************************************************/
uint32_t Curve_Sport_Close(func_StepMotor_t *step_motor)
{
	step_motor->res_set_run_control(0);
	Clearing_SportCurve_value(step_motor);

	step_motor->private.step_status = STEP_MOTOR_STOP;
	return 0;
}

/***********************************************************************************
 * @brief 扫描运动曲线
 * ex:          curve_count 和step_motor->rel_pulse_tar 控制脉冲输出，关键变量     
 * @par         step_motor->rel_detec_pulse_tar  和step_motor->pulse_cur  实时变化，但只是观测变量
 * None
 * @retval 优化思路：通过函数指针来替代if判断
 **********************************************************************************/
void Scan_SportCurve(func_StepMotor_t *step_motor)
{
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;
	if (step_motor->step_workmode == STEP_NO_FEEDBACK_RUN) //不带反馈控制
	{
		if (acc->sport_type == S_ACCELERATE_SPORT) //带加减速运动过程
		{
			if (acc->curve_count < acc->ACC_Curve_Sum) //加速300脉冲  0~299
			{
				acc->curve_count += S_AccCurve_Sport(step_motor);
			}
			else if (acc->curve_count <
					 (step_motor->private.rel_pulse_tar - acc->ACC_Curve_Sum)) //匀速100脉冲  300~399
			{
				acc->curve_count += UniformCurve_Sport(step_motor);
			}
			else if (acc->curve_count < step_motor->private.rel_pulse_tar) //减速200脉冲  400~699
			{
				acc->curve_count += S_ReduceCurve_Sport(step_motor);
			}
			else
			{
				Curve_Sport_Close(step_motor);
			}
		}
		else if (acc->sport_type == UNIFORM_SPORT) //匀速运动
		{
			if (acc->curve_count < step_motor->private.rel_pulse_tar)
			{
				acc->curve_count += UniformCurve_Sport(step_motor);
			}
			else
			{
				Curve_Sport_Close(step_motor);
			}
		}
	}
}

void step_run_table_init(func_StepMotor_t *step_motor)
{
	MotorAccPar_Struct_t *acc = &step_motor->private.MotorAcc;
	SPlotAdaptParaDef_t *P_spl = NULL;

	P_spl = SPlotAccSpeedAdapt_Search(SPlotAccSpeedAdapt_List, step_motor->speed);
	acc->ACC_flexible = P_spl->flexible;
	acc->ACC_start_speed = P_spl->start_speed;
	acc->ACC_Curve_Sum = P_spl->sum;

	P_spl = SPlotReduceSpeedAdapt_Search(SPlotReduceSpeedAdapt_List, step_motor->speed);
	acc->Reduce_flexible = P_spl->flexible;
	acc->Reduce_end_speed = P_spl->start_speed;
	acc->Reduce_Curve_Sum = P_spl->sum;

	Plot_S_AccCurve(step_motor, step_motor->speed);
	Plot_S_ReduceCurve(step_motor, step_motor->speed);
}

/******************* (C) COPYRIGHT 2022 Moon *************END OF FILE****/

